#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/publisher.h>
#include <sensor_msgs/msg/laser_scan.h>
#include <rtthread.h>
#include <rtdevice.h>
#include <board.h>
#include <rplidar.h>
#include <math.h>
#include <string.h>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#define LIDAR_SCAN_COUNT     360
#define LIDAR_TIMEOUT_MS     100
#define LASER_PUB_PERIOD_MS  100

static rcl_publisher_t laser_pub;
static sensor_msgs__msg__LaserScan scan_msg;
static float scan_ranges[LIDAR_SCAN_COUNT];
static rt_device_t lidar = RT_NULL;

static void publish_laserscan()
{
    rplidar_response_measurement_node_t node;
    float angle, distance;
    int count = 0;

    memset(scan_ranges, 0, sizeof(scan_ranges));

    for (int i = 0; i < LIDAR_SCAN_COUNT; i++)
    {
        if (rp_lidar_get_scan_data(lidar, &node, LIDAR_TIMEOUT_MS) != RT_EOK)
        {
            rt_kprintf("Warning: Scan timeout\n");
            continue;
        }

        // 解码
        angle = (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f;
        distance = node.distance_q2 / 4.0f; // mm → cm

        if (distance > 0 && angle >= 0.0f && angle < 360.0f)
        {
            int index = (int)angle;
            scan_ranges[index] = distance / 1000.0f; // cm → m
            count++;
        }
    }

    scan_msg.header.stamp.sec = rt_tick_get() / RT_TICK_PER_SECOND;
    scan_msg.header.stamp.nanosec = (rt_tick_get() % RT_TICK_PER_SECOND) * 1000000;
    scan_msg.ranges.data = scan_ranges;
    scan_msg.ranges.size = LIDAR_SCAN_COUNT;

    rcl_publish(&laser_pub, &scan_msg, NULL);
}

static void ros_lidar_thread(void *arg)
{
    rcl_allocator_t allocator = rcl_get_default_allocator();
    rclc_support_t support;
    rcl_node_t node;

    rclc_support_init(&support, 0, NULL, &allocator);
    rclc_node_init_default(&node, "laser_node", "", &support);

    rclc_publisher_init_default(
        &laser_pub,
        &node,
        ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, LaserScan),
        "scan");

    scan_msg.angle_min = 0.0f;
    scan_msg.angle_max = 2 * M_PI;
    scan_msg.angle_increment = 2 * M_PI / LIDAR_SCAN_COUNT;
    scan_msg.time_increment = 0.0f;
    scan_msg.scan_time = 0.1f;
    scan_msg.range_min = 0.15f;
    scan_msg.range_max = 6.0f;

    scan_msg.header.frame_id.data = "laser";
    scan_msg.header.frame_id.size = strlen("laser");
    scan_msg.header.frame_id.capacity = 10;
    scan_msg.ranges.capacity = LIDAR_SCAN_COUNT;

    while (1)
    {
        publish_laserscan();
        rt_thread_mdelay(LASER_PUB_PERIOD_MS);
    }
}

void start_ros_lidar(void)
{
    lidar = rp_lidar_create("uart3");
    if (lidar == RT_NULL)
    {
        rt_kprintf("Error: RPLIDAR not found!\n");
        return;
    }

    if (rp_lidar_init(lidar) != RT_EOK)
    {
        rt_kprintf("Error: LIDAR init failed!\n");
        return;
    }

    if (rp_lidar_scan(lidar, 1000) != RT_EOK)
    {
        rt_kprintf("Error: Start scan failed!\n");
        return;
    }

    rt_kprintf("RPLIDAR scanning started...\n");

    rt_thread_t thread = rt_thread_create("ros_lidar",
                                          ros_lidar_thread,
                                          RT_NULL,
                                          4096, 20, 10);
    if (thread)
        rt_thread_startup(thread);
}

